#include "led.hpp"
#include "driver/gpio.h"
#include "hal/gpio_types.h"

#include <stdio.h>

#define LED_ON  0
#define LED_OFF 1

Led::Led(gpio_num_t pinFrontLeft, gpio_num_t pinFrontRight, gpio_num_t pinBackLeft, gpio_num_t pinBackRight, uint32_t onLevel) {

    this->pinFrontLeft  = pinFrontLeft;
    this->pinFrontRight = pinFrontRight;
    this->pinBackLeft   = pinBackLeft;
    this->pinBackRight  = pinBackRight;
    this->onLevel       = onLevel;

    this->init();
}

void Led::init()
{
    // 直接设置为推挽输出，然后set level不可以，可能上下拉并不是默认disable
    // gpio_set_direction(LED_PIN_FRONT_LEFT, GPIO_MODE_OUTPUT);
    // gpio_set_direction(LED_PIN_FRONT_RIGHT, GPIO_MODE_OUTPUT);
    // gpio_set_direction(LED_PIN_BACK_LEFT, GPIO_MODE_OUTPUT);
    // gpio_set_direction(LED_PIN_BACK_RIGHT, GPIO_MODE_OUTPUT);

    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << this->pinFrontLeft) |
                        (1ULL << this->pinFrontRight) |
                        (1ULL << this->pinBackLeft) |
                        (1ULL << this->pinBackRight),
        .mode         = GPIO_MODE_OUTPUT,
        .pull_up_en   = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type    = GPIO_INTR_DISABLE
    };

    gpio_config(&io_conf);

    this->allOn();
}

bool Led::validatePin(gpio_num_t pin)
{
    return pin == this->pinFrontLeft
        || pin == this->pinFrontRight
        || pin == this->pinBackLeft
        || pin == this->pinBackRight;
}

void Led::on(gpio_num_t pin)
{
    if (this->validatePin(pin)) {
        gpio_set_level(pin, LED_ON);
    }
}

void Led::off(gpio_num_t pin)
{
    if (this->validatePin(pin)) {
        gpio_set_level(pin, LED_OFF);
    }
}

void Led::toggle(gpio_num_t pin)
{
    if (this->validatePin(pin)) {
        // if (LED_OFF == gpio_get_level(pin)) {
        //     this->on(pin);
        // } else {
        //     this->off(pin);
        // }

        // 很奇怪，gpio_get_level 永远为0，所以不得不用以下方式

        int index = 0;
        if (pin == this->pinFrontLeft) {
            index = 0;
        } else if (pin == this->pinFrontRight) {
            index = 1;
        } else if (pin == this->pinBackLeft) {
            index = 2;
        } else if (pin == this->pinBackRight) {
            index = 3;
        }

        static bool toggled[4] = {false, false, false, false};
        if (toggled[index]) {
            this->off(pin);
            toggled[index] = false;
        } else {
            this->on(pin);
            toggled[index] = true;
        }
    }
}

void Led::allOn()
{
    this->on(this->pinFrontLeft);
    this->on(this->pinFrontRight);
    this->on(this->pinBackLeft);
    this->on(this->pinBackRight);
}

void Led::allOff()
{
    this->off(this->pinFrontLeft);
    this->off(this->pinFrontRight);
    this->off(this->pinBackLeft);
    this->off(this->pinBackRight);
}

void Led::allToggle()
{
    this->toggle(this->pinFrontLeft);
    this->toggle(this->pinFrontRight);
    this->toggle(this->pinBackLeft);
    this->toggle(this->pinBackRight);
}
